/*
 * eskf.hpp
 *
 *  Created on: 2020年12月3日
 *      Author: 刘成吉
 */

#ifndef MODULES_ESKF_ESKF_HPP_
#define MODULES_ESKF_ESKF_HPP_

#include "system/system.hpp"
#include "userlib/userlib.hpp"

#ifdef __cplusplus
#include <Eigen>
#include <malloc.h>
#include "MySeqQueue.hpp"
#include "mahanyCPF.hpp"
using namespace std;
using namespace Eigen;

#define scalar_g	9.7883
#define dt	0.01f

//100Hz，用于XY轴
#define a0_2nd 0.067455273889071895587754568168747937307f
#define a1_2nd 0.134910547778143791175509136337495874614f
#define a2_2nd 0.067455273889071895587754568168747937307f
#define b1_2nd -1.142980502539901133118860343529377132654f
#define b2_2nd   0.412801598096188770981029847462195903063f

//50Hz，用于Z轴
#define a0_2nd2 0.02008336556421123561544384017452102853f
#define a1_2nd2 0.04016673112842247123088768034904205706f
#define a2_2nd2 0.02008336556421123561544384017452102853f
#define b1_2nd2 -1.561018075800718163392843962355982512236f
#define b2_2nd2   0.641351538057563175243558362126350402832f

class MAHANYCPF;

typedef struct
{
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
	Quaternion<float> q;
	Vector3f Pos;		//单位：弧度rad&米m
	Vector3f Vned;
	Vector3f Ba;
	Vector3f Bg;
}STATES;	//64字节长度

typedef struct
{
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
	Vector3f wm;		//imu测量值(角速度)
	Vector3f accm;		//加速度
	Vector3f magm;		//磁力计测量(假设磁力计与imu同步更新)
	uint32_t updtime;		//更新时间
}IMUMag;

typedef struct
{
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
	Vector3f Pos;			//单位：m
	Vector3f Vned;
	uint32_t updtime;		//更新时间
}GPS;

class ESKF
{
private:
/*系统状态x = [P V_ned q beta_a beta_g]',     Δx' = F·Δx + G·w
  *先把所有16Bytes倍数长度的变量放前面一块，每个变量相对于类首的相对地址又必须是16的整数倍(16字节倍数长度的成员变量按16字节对齐，包括32字节长度倍数)
 */
	Quaternion<float> q;
	Matrix<float,12,12> Q;
	Matrix<float,15,12> G;
	STATES stateTempEle;

/*
 *4字节长度倍数（非16字节倍数）长度变脸按4字节对齐（包括8字节）
 */
	Matrix<float,15,15> P;
	Matrix<float,15,15> F;
	Matrix<float,15,15> QQ;
	Matrix<float,7,15> H;
	Matrix<float,15,7> K;
	Matrix<float,7,7> R;
	Matrix<float,15,1> delta_x;	//Δx = x - x_estimate
	Matrix<float,7,1> z_sub_y;
	Matrix<float,3,3> A;	//旋转矩阵
	Matrix3f FS_matrix;		//本体坐标切换矩阵
	Matrix3f drotation;
	Matrix<float,3,3> I3;

	Vector3f maglevel;		//水平机头坐标系下的磁场
	Vector3f Pos;			//北东地位置
	Vector3f Vned;
	Vector3f Ba;
	Vector3f Bg;

	Vector3f g;
	Vector3f accm;			//加速度计测量值
	Vector3f wm;			//陀螺仪测量值
	Vector3f accb;			//accb = accm - Ba;
	Vector3f accn;			//accn = A*Accb;
	Vector3f wb;			//wb = wm - Bg;
    //观测数据：
	Vector3f magm;			//磁力计测量值
	Vector3f Ang; 		    //欧拉角，roll, pitch, yaw

	//互补滤波部分变量
	Vector3f pos_err;
	Vector3f pos_err_inter;
	Vector3f vel_err;
	Vector3f vel_err_inter;
	Vector3f ang_err;
	Vector3f ang_err_inter;
	Vector3f pos_cor;
	Vector3f vel_cor;
	Vector3f ang_cor;

	//杆臂补偿
	Vector3f GpsInsPosErr; //GPS芯片相对于IMU来说的距离

	float psim;						//测量偏航角
	float dpsim;					//偏航角误差
	uint32_t  startTimer;			//计时器
	uint32_t  stopTimer;			//计时器
	uint32_t  executionTime_us;			//计时器
	uint32_t  startTimerLast;			//计时器
	uint32_t  cycleTime_us;

	//读取传感器数据
	gps_msg gps;
//	sensor_gyro_msg gyro;
//	sensor_acc_msg acc;
	downsample_imu_for_eskf_msg imu;
	mag_msg mag;
	eskf_msg output;

	IMUMag ImuMagTempEle;
	GPS GpsTempEle;

	MySeqQueue<STATES> states_Q;	//状态队列：四元数、纬经高、NED速度
	MySeqQueue<IMUMag> imu_Q;		//imu队列：角加速度、加速度、磁力计、更新时间
	GPS gps_Q;						//gps：北东地、NED速度、更新时间（始终用最新的GPS，假设延时时间固定420ms）

	PID_msg pid;//调试互补滤波写参数用

	//杆臂补偿数据
	Matrix<float,3,3> wmCrossMul;
	Vector3f GpsPosTemp;
	Vector3f GpsVelTemp;
	Matrix<float,3,3> rawA;	//由于GPS存在300ms的时延，而A这个旋转矩阵是实时的，因此需要用300ms之前的A进行计算，也就是在states_Q的队头
	float Gyro300ms[3] = {0.0f, 0.0f, 0.0f};
	float Gyro300msLast[3] = {0.0f, 0.0f, 0.0f};
	float Gyro300msLastLast[3] = {0.0f, 0.0f, 0.0f};
	float Gyro300msFilter[3] = {0.0f, 0.0f, 0.0f};
	float Gyro300msFilterLast[3] = {0.0f, 0.0f, 0.0f};
	float Gyro300msFilterLastLast[3] = {0.0f, 0.0f, 0.0f};
	STATES stateTemp;
	IMUMag ImuMagTemp;

	/*
	 * 调试用到的变量临时存放到下面区域
	 */

public:
	EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
	ESKF(int stateQ_size = 28):states_Q(stateQ_size), imu_Q(stateQ_size){}; //200Hz 28->56
//	ESKF(){};
	~ESKF(){};

	void init(const MAHANYCPF& mahany);
	void Eskf_Calc();
	void propagate();
	void update();
	//互补滤波部分
	bool readSensors();
	void refreshStatesQ();	//互补滤波
	void outputStates();
	void GyrFilter();
	RC_command_msg rcCommand;
};




#endif
#endif /* MODULES_ESKF_ESKF_HPP_ */
